#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <cmath>

class CarrotPlanner {
public:
    CarrotPlanner() {
        // 订阅机器人当前位姿和目标位姿
        current_pose_sub_ = nh_.subscribe("/current_pose", 1, &CarrotPlanner::currentPoseCallback, this);
        goal_pose_sub_ = nh_.subscribe("/goal_pose", 1, &CarrotPlanner::goalPoseCallback, this);

        // 发布规划出的下一个目标点
        next_pose_pub_ = nh_.advertise<geometry_msgs::PoseStamped>("/next_pose", 1);
    }

    // 当接收到机器人当前位姿的回调函数
    void currentPoseCallback(const geometry_msgs::PoseStamped::ConstPtr &msg) {
        current_pose_ = *msg;
        if (current_pose_set_ && goal_pose_set_) {
            planNextPose();
        }
    }

    // 当接收到目标位姿的回调函数
    void goalPoseCallback(const geometry_msgs::PoseStamped::ConstPtr &msg) {
        goal_pose_ = *msg;
        goal_pose_set_ = true;
        if (current_pose_set_) {
            planNextPose();
        }
    }

    // 规划下一个目标点
    void planNextPose() {
        double distance = sqrt(pow(goal_pose_.pose.position.x - current_pose_.pose.position.x, 2) +
                               pow(goal_pose_.pose.position.y - current_pose_.pose.position.y, 2));
        if (distance <= carrot_distance_) {
            // 目标点可达，直接发布
            next_pose_pub_.publish(goal_pose_);
        } else {
            // 目标点不可达，沿着当前点到目标点的直线上找一个可达点
            double delta_x = goal_pose_.pose.position.x - current_pose_.pose.position.x;
            double delta_y = goal_pose_.pose.position.y - current_pose_.pose.position.y;
            double theta = atan2(delta_y, delta_x);
            double next_x = current_pose_.pose.position.x + carrot_distance_ * cos(theta);
            double next_y = current_pose_.pose.position.y + carrot_distance_ * sin(theta);
            geometry_msgs::PoseStamped next_pose;
            next_pose.header.frame_id = current_pose_.header.frame_id;
            next_pose.header.stamp = ros::Time::now();
            next_pose.pose.position.x = next_x;
            next_pose.pose.position.y = next_y;
            next_pose.pose.orientation = tf2::toMsg(tf2::Quaternion::getIdentity());
            next_pose_pub_.publish(next_pose);
        }
    }

private:
    ros::NodeHandle nh_;
    ros::Subscriber current_pose_sub_;
    ros::Subscriber goal_pose_sub_;
    ros::Publisher next_pose_pub_;
    geometry_msgs::PoseStamped current_pose_;
    geometry_msgs::PoseStamped goal_pose_;
    bool current_pose_set_ = false;
    bool goal_pose_set_ = false;
    double carrot_distance_ = 0.5;  // 距离目标点的最小距离
};

int main(int argc, char **argv) {
    ros::init(argc, argv, "cre_carrot_planner");
    CarrotPlanner planner;
    ros::spin();
    return 0;
}